#include <stdio.h>
#include <string.h>
#include <inttypes.h>

#include "ethercat.h"

#define NSEC_PER_SEC 1000000000
#define EC_TIMEOUTMON 500

char IOmap[4096];
int expectedWKC;
boolean needlf;
volatile int wkc;
int CYCTIME_MS = 16;

int states[50];

/*****************************************************************************/
/**
   statusword bit definition.
*/
typedef struct _stswrd_bits_t
{
  uint8_t rtso : 1; // BIT0: ready to switch on
  uint8_t so : 1;   // BIT1: switched on
  uint8_t oe : 1;   // BIT2: operation enabled
  uint8_t f : 1;    // BIT3: fault
  uint8_t ve : 1;   // BIT4:  voltage enabled
  uint8_t qs : 1;   // BIT5: quick stop
  uint8_t sod : 1;  // BIT6: switch on disabled
  uint8_t w : 1;    // BIT7: warning
  uint8_t ms1 : 1;  // BIT8: manufacturer-specific1
  uint8_t rm : 1;   // BIT9: remote
  uint8_t tr : 1;   // BIT10: target reached
  uint8_t ila : 1;  // BIT11:  internal limit active
  uint8_t oms1 : 1; // BIT12: oms1 pp-Set-point acknowledge; hm-Homing attained;
                    //		ip-ip  mode active;csp-Target position ignored;
                    //		csv-Target velocity ignored;cst-Target torque ignored;
  uint8_t oms2 : 1; // BIT13: oms1 pp-Following error; hm-Homing error; csp-Following error;
  uint8_t ms2 : 1;  // BIT14: manufacturer-specific2
  uint8_t ms3 : 1;  // BIT15: manufacturer-specific3
} stswrd_bits_t;
/*****************************************************************************/
/**
   statusword definition.
*/
typedef union _stswrd_t
{
  uint16_t all;
  stswrd_bits_t bit;
} stswrd_t;
/*****************************************************************************/

typedef struct PACKED
{
  uint16_t Controlword;
  int32_t TargetPos;
  int32_t TargetVel;
  int16_t TargetTor;
  uint8_t ModeOp;
} Drive_Outputs;

typedef struct PACKED
{
  uint16_t Statusword;
  int32_t ActualPos;
  int32_t ActualVel;
  int16_t ActualTor;
  uint8_t ModeOp;
} Drive_Inputs;

typedef struct PACKED
{
  uint16_t Controlword;
  uint16_t Return_Request;
  uint32_t Out_test;

  uint8_t DO_1;
  uint8_t DO_2;
  uint8_t DO_3;
  uint8_t DO_4;
  uint8_t DO_5;
  uint8_t DO_6;
  uint8_t DO_7;
  uint8_t DO_8;
  uint8_t DO_9;
  uint8_t DO_10;
  uint8_t DO_11;
  uint8_t DO_12;
  uint8_t DO_13;
  uint8_t DO_14;
  uint8_t DO_15;
  uint8_t DO_16;


} Drive_Outputs_IO;

typedef struct PACKED
{
  uint16_t SM_states;
  uint32_t IO_Inputs;
  uint16_t Request_Code;
  uint16_t Error_Code;
  uint16_t Master_Modes;
  uint32_t In_test;

  uint8_t DI_1;
  uint8_t DI_2;
  uint8_t DI_3;
  uint8_t DI_4;
  uint8_t DI_5;
  uint8_t DI_6;
  uint8_t DI_7;
  uint8_t DI_8;
  uint8_t DI_9;
  uint8_t DI_10;
  uint8_t DI_11;
  uint8_t DI_12;
  uint8_t DI_13;
  uint8_t DI_14;
  uint8_t DI_15;
  uint8_t DI_16;

} Drive_Inputs_IO;



static int drive_write8(uint16 slave, uint16 index, uint8 subindex, uint8 value)
{
  int wkc;

  wkc = ec_SDOwrite(slave, index, subindex, FALSE, sizeof(value), &value, EC_TIMEOUTRXM);

  return wkc;
}

static int drive_write16(uint16 slave, uint16 index, uint8 subindex, uint16 value)
{
  int wkc;

  wkc = ec_SDOwrite(slave, index, subindex, FALSE, sizeof(value), &value, EC_TIMEOUTRXM);

  return wkc;
}

static int drive_write32(uint16 slave, uint16 index, uint8 subindex, int32 value)
{
  int wkc;

  wkc = ec_SDOwrite(slave, index, subindex, FALSE, sizeof(value), &value, EC_TIMEOUTRXM);

  return wkc;
}

int drive_setup_IO(uint16 slave)
{
  int wkc = 0;

  printf("Drive setup\n");

  wkc += drive_write16(slave, 0x1C12, 0, 0);
  printf("wkc %d \n", wkc);
  wkc += drive_write16(slave, 0x1C13, 0, 0);
  printf("wkc %d \n", wkc);
  wkc += drive_write16(slave, 0x1A00, 0, 0);
  printf("wkc %d \n", wkc);
  int idx = 0;

  idx++;
  wkc += drive_write32(slave, 0x1A00, idx, 0x60010010);
  printf("wkc %d \n", wkc);
  idx++;
  wkc += drive_write32(slave, 0x1A00, idx, 0x60020020);
  idx++;
  wkc += drive_write32(slave, 0x1A00, idx, 0x60030010);
  idx++;
  wkc += drive_write32(slave, 0x1A00, idx, 0x60040010);
  idx++;
  wkc += drive_write32(slave, 0x1A00, idx, 0x60050010);
  idx++;
  wkc += drive_write32(slave, 0x1A00, idx, 0x60060020);

  idx++;
  wkc += drive_write32(slave, 0x1A00, idx, 0x60070101);
  // idx++;
  // wkc += drive_write32(slave, 0x1A00, idx, 0x60070201);
  // idx++;
  // wkc += drive_write32(slave, 0x1A00, idx, 0x60070301);
  // idx++;
  // wkc += drive_write32(slave, 0x1A00, idx, 0x60070401);

  // idx++;
  // wkc += drive_write32(slave, 0x1A00, idx, 0x60070501);
  // idx++;
  // wkc += drive_write32(slave, 0x1A00, idx, 0x60070601);
  // idx++;
  // wkc += drive_write32(slave, 0x1A00, idx, 0x60070601);
  // idx++;
  // wkc += drive_write32(slave, 0x1A00, idx, 0x60070801);

  // idx++;
  // wkc += drive_write32(slave, 0x1A00, idx, 0x60070901);
  // idx++;
  // wkc += drive_write32(slave, 0x1A00, idx, 0x60070A01);
  // idx++;
  // wkc += drive_write32(slave, 0x1A00, idx, 0x60070B01);
  // idx++;
  // wkc += drive_write32(slave, 0x1A00, idx, 0x60070C01);

  // idx++;
  // wkc += drive_write32(slave, 0x1A00, idx, 0x60070D01);
  // idx++;
  // wkc += drive_write32(slave, 0x1A00, idx, 0x60070E01);
  // idx++;
  // wkc += drive_write32(slave, 0x1A00, idx, 0x60070F01);
  // idx++;
  // wkc += drive_write32(slave, 0x1A00, idx, 0x60071001);

  wkc += drive_write8(slave, 0x1A00, 0, idx);

  wkc += drive_write8(slave, 0x1601, 0, 0);

  int idx2 = 0; // 从索引1开始

  idx2++;
  wkc += drive_write32(slave, 0x1601, idx2, 0x70100010);
  idx2++;
  wkc += drive_write32(slave, 0x1601, idx2, 0x70110010);
  idx2++;
  wkc += drive_write32(slave, 0x1601, idx2, 0x70120020);

  idx2++;
  wkc += drive_write32(slave, 0x1601, idx2, 0x70130101);
  // idx2++;
  // wkc += drive_write32(slave, 0x1601, idx2, 0x70130201);
  // idx2++;
  // wkc += drive_write32(slave, 0x1601, idx2, 0x70130301);
  // idx2++;
  // wkc += drive_write32(slave, 0x1601, idx2, 0x70130401);

  // idx2++;
  // wkc += drive_write32(slave, 0x1601, idx2, 0x70130501);
  // idx2++;
  // wkc += drive_write32(slave, 0x1601, idx2, 0x70130601);
  // idx2++;
  // wkc += drive_write32(slave, 0x1601, idx2, 0x70130701);
  // idx2++;
  // wkc += drive_write32(slave, 0x1601, idx2, 0x70130801);

  // idx2++;
  // wkc += drive_write32(slave, 0x1601, idx2, 0x70130901);
  // idx2++;
  // wkc += drive_write32(slave, 0x1601, idx2, 0x70130A01);
  // idx2++;
  // wkc += drive_write32(slave, 0x1601, idx2, 0x70130B01);
  // idx2++;
  // wkc += drive_write32(slave, 0x1601, idx2, 0x70130C01);
  
  // idx2++;
  // wkc += drive_write32(slave, 0x1601, idx2, 0x70130D01);
  // idx2++;
  // wkc += drive_write32(slave, 0x1601, idx2, 0x70130E01);
  // idx2++;
  // wkc += drive_write32(slave, 0x1601, idx2, 0x70130F01);
  // idx2++;
  // wkc += drive_write32(slave, 0x1601, idx2, 0x70131001);

  wkc += drive_write8(slave, 0x1601, 0, idx2);

  wkc += drive_write16(slave, 0x1C12, 1, 0x1601);
  wkc += drive_write8(slave, 0x1C12, 0, 1);

  wkc += drive_write16(slave, 0x1C13, 1, 0x1A00);
  wkc += drive_write8(slave, 0x1C13, 0, 1);

  strncpy(ec_slave[slave].name, "Drive", EC_MAXNAME);

  if (wkc != 10 + idx + idx2)
  {
    printf("Drive %d setup failed\nwkc: %d\n", slave, wkc);
    return -1;
  }
  else
    printf("Drive %d setup succeed.\n", slave);

  return 0;
}

int drive_setup(uint16 slave)
{
  int wkc = 0;

  printf("Drive setup\n");

  wkc += drive_write16(slave, 0x1C12, 0, 0);
  wkc += drive_write16(slave, 0x1C13, 0, 0);

  wkc += drive_write16(slave, 0x1A00, 0, 0);
  wkc += drive_write32(slave, 0x1A00, 1, 0x60410010); // Statusword
  wkc += drive_write8(slave, 0x1A00, 0, 1);

  wkc += drive_write8(slave, 0x1601, 0, 0);
  wkc += drive_write32(slave, 0x1601, 1, 0x60400010); // Controlword
  wkc += drive_write8(slave, 0x1601, 0, 1);

  wkc += drive_write16(slave, 0x1C12, 1, 0x1601);
  wkc += drive_write8(slave, 0x1C12, 0, 1);

  wkc += drive_write16(slave, 0x1C13, 1, 0x1A00);
  wkc += drive_write8(slave, 0x1C13, 0, 1);

  strncpy(ec_slave[slave].name, "Drive", EC_MAXNAME);

  if (wkc != 12)
  {
    printf("Drive %d setup failed\nwkc: %d\n", slave, wkc);
    return -1;
  }
  else
    printf("Drive %d setup succeed.\n", slave);

  return 0;
}

/* add ns to timespec */
void add_timespec(struct timespec *ts, int64 addtime)
{
  int64 sec, nsec;

  nsec = addtime % NSEC_PER_SEC;
  sec = (addtime - nsec) / NSEC_PER_SEC;
  ts->tv_sec += sec;
  ts->tv_nsec += nsec;
  if (ts->tv_nsec > NSEC_PER_SEC)
  {
    nsec = ts->tv_nsec % NSEC_PER_SEC;
    ts->tv_sec += (ts->tv_nsec - nsec) / NSEC_PER_SEC;
    ts->tv_nsec = nsec;
  }
}

/* PI calculation to get linux time synced to DC time */
void ec_sync(int64 reftime, int64 cycletime, int64 *offsettime)
{
  static int64 integral = 0;
  int64 delta;
  /* set linux sync point 50us later than DC sync, just as example */
  delta = (reftime - 50000) % cycletime;
  if (delta > (cycletime / 2))
  {
    delta = delta - cycletime;
  }
  if (delta > 0)
  {
    integral++;
  }
  if (delta < 0)
  {
    integral--;
  }
  *offsettime = -(delta / 100) - (integral / 20);
}

static inline int64_t calcdiff_ns(struct timespec t1, struct timespec t2)
{
  int64_t tdiff;
  tdiff = NSEC_PER_SEC * (int64_t)((int)t1.tv_sec - (int)t2.tv_sec);
  tdiff += ((int)t1.tv_nsec - (int)t2.tv_nsec);
  return tdiff;
}

void simple_test(char *ifname)
{
  if (ec_init(ifname))
  {
    printf("SOEM init on %s succeeded.\n", ifname);

    if (ec_config_init(FALSE) > 0)
    {
      printf("%d slaves found and configured.\n", ec_slavecount);

      int slave_ix;
      for (slave_ix = 1; slave_ix <= ec_slavecount; slave_ix++)
      {
        ec_slavet *slave = &ec_slave[slave_ix];
        printf("Found %s at position %d\n", ec_slave[slave_ix].name, slave_ix);
          // slave->PO2SOconfig = drive_setup_IO;
      }

      ec_config_map(&IOmap);
      for (slave_ix = 1; slave_ix <= ec_slavecount; slave_ix++)
      {
          // ec_dcsync0(slave_ix, TRUE, CYCTIME_MS * 1000000U , 20000U);
            ec_dcsync0(slave_ix, TRUE, CYCTIME_MS * 1000000U , CYCTIME_MS * 1000000U / 4);
      }
      ec_configdc();

      printf("Slaves mapped, state to SAFE_OP.\n");
      /* wait for all slaves to reach SAFE_OP state */
      ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE * 4);
      expectedWKC = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC;
      printf("Calculated workcounter %d\n", expectedWKC);

      ec_slave[0].state = EC_STATE_OPERATIONAL;
      ec_writestate(0);


      int chk = 200;
      /* wait for all slaves to reach OP state */
      do
      {
        osal_usleep( CYCTIME_MS * 1000);
        wkc = ec_receive_processdata(EC_TIMEOUTRET);
        ec_send_processdata();
        // ec_statecheck(0, EC_STATE_OPERATIONAL, 50000);
        printf("chk: %d \n", chk);
      } while (chk-- && ( (ec_slave[0].state != EC_STATE_OPERATIONAL) || (wkc != expectedWKC) ));

       Drive_Inputs_IO *iptr;
       Drive_Outputs_IO *optr;

      if (ec_slave[0].state == EC_STATE_OPERATIONAL && wkc == expectedWKC)
      {
        printf("Operational state reached for all slaves.\n");
        int cnt = 0;
        while (2 > 1)
        {
          cnt++;
          if (cnt > 100)
            cnt = 0;

         
          iptr = (Drive_Inputs_IO *)ec_slave[1].inputs;          
          optr = (Drive_Outputs_IO *)ec_slave[1].outputs;
          if (cnt == 1)
            optr->DO_1 = 1;
          if (cnt == 50)
            optr->DO_1 = 0;

          printf("ec_slave[1].inputs: %d \n", (int)(iptr->DI_1));

          osal_usleep( CYCTIME_MS * 1000);
          wkc = ec_receive_processdata(EC_TIMEOUTRET);
          ec_send_processdata();
        }
      }
      else
      {
        printf("Not all slaves reached operational state.\n");
      }
    }
    else
    {
      printf("No slaves found!\n");
    }
  }
  else
  {
    printf("SOEM init failed!\n");
  }
}

int main(int argc, char *argv[])
{
  printf("SOEM EtherCAT IO Example (Windows)\n");

  if (argc > 1)
  {
    simple_test(argv[1]);
  }
  else
  {
    printf("Usage: %s <ifname>\n", argv[0]);
    printf("Example: %s \"\\Device\\NPF_{XXXXXXXX-XXXX-XXXX-XXXX-XXXXXXXXXXXX}\"\n", argv[0]);
  }

  printf("End program\n");
  return 0;
}
